#include <library.h>

double interpolate_at(double x, double y,
	double x1, double x2, double y1, double y2,
	double q11, double q12, double q21, double q22 ){

	double dx = x2 - x1;
	double dy = y2 - y1;

	double r2 = ( (x2-x) * q11 + (x-x1) * q21 )/dx;
	double r1 = ( (x2-x) * q12 + (x-x1) * q22 )/dx;

	return ((y2-y) * r1 + (y-y1) * r2 )/dy;
}

double get_yaw(geometry_msgs::Quaternion qtr){
	tf::Quaternion tqtr( qtr.x, qtr.y, qtr.z, qtr.w	);
	return tf::getYaw(tqtr);
}

double get_yaw(tf::StampedTransform & transform){
	tf::Quaternion tqtr(
		transform.getRotation().x(),
		transform.getRotation().y(),
		transform.getRotation().z(),
		transform.getRotation().w()
	);
	return tf::getYaw(tqtr);
}

double wave_front_marcher(VECTOR &v, double F){
	double Ta = INF, Tc = INF;
	int sz = v.size();

	for(int i=0; i<sz;i++)
		Ta = std::min(v[i],Ta);

	for(int i=0;i<sz;i++)
		if(v[i] != Ta)
			Tc = std::min(v[i],Tc);

	assert(Ta != INF);

	if( Tc == INF ){
		return Ta + F;
	}else{
		double t = 2 * F * F - (( Ta - Tc )*( Ta - Tc ));
		if( t >= 0 ) {
			t = sqrt(t);
			return (Ta+Tc+t)/2.0;
		}
		return Ta + F;
	}
}

PII index(double x,double y,double grain){
	int ix = floor( y/grain );
	int iy = floor( x/grain );

	return PII(ix,iy);
}

DD unindex(int x,int y, double grain){
	double xx = y * grain;
	double yy = x * grain;
	return DD(xx,yy);
}
